#ifndef URCONTROLPLANE_HPP
#define URCONTROLPLANE_HPP

#include "urcontrol.hpp"
#include "imagetoplane.hpp"

class URControlPlane : public ImageToPlane, public URControl
{
public:

    URControlPlane()
        : ImageToPlane()
        , URControl()
    {
        //        ImageToPlane::test();


        cv::Vec6d headPosBase[4]={
            cv::Vec6d(0.244364,-0.179697,0.221591,-1.09127,2.90713,0.183697),
            cv::Vec6d(0.443805,-0.144863,0.225187,-1.49533,2.59919,-0.258374),
            cv::Vec6d(0.415067,0.212753,0.219907,-1.23241,2.70302,-0.296297),
            cv::Vec6d(0.222613,0.175098,0.220047,-1.10212,2.87574,0.0419509)
        };

        cv::Vec2d referencePixel[4]={
            cv::Vec2d(430,36),
            cv::Vec2d(121,24),
            cv::Vec2d(142,470),
            cv::Vec2d(436,471)
        };

        ImageToPlane::Calibrate(cv::Vec3d(0,0,0.21), headPosBase, referencePixel, cv::Size(640,480), false);
    }


    void PickRectangleCenter(cv::Vec2d rectangleCorner[4], const char *ipUR, const char *ipPC, double openmm,
    double v=0.2, double a=0.1, cv::Vec3d fingerXYZ=cv::Vec3d(0,0,0.21))
    {
        cv::Vec6d pos=ImageToPlane::RectangleCornerPixelCoordinateToHeadPos(fingerXYZ, rectangleCorner);

        URControl::SetTCPPose(ipUR, ipPC, pos[0],pos[1],pos[2]+0.06,pos[3],pos[4],pos[5], a, v);

        URControl::RG2(ipUR, ipPC, openmm);

        URControl::SetTCPPose(ipUR, ipPC, pos[0],pos[1],pos[2]+0.02,pos[3],pos[4],pos[5], a, v);

        URControl::RG2(ipUR, ipPC, 0);

        URControl::SetTCPPose(ipUR, ipPC, pos[0],pos[1],pos[2]+0.06,pos[3],pos[4],pos[5], a, v);

        cv::Vec6d droppos(0,0,0,0,0,0);

        URControl::SetTCPPose(ipUR, ipPC, droppos[0],droppos[1],droppos[2],droppos[3],droppos[4],droppos[5], a, v);

        URControl::RG2(ipUR, ipPC, openmm);

        cv::Vec6d idlepos(0,0,0,0,0,0);

        URControl::SetTCPPose(ipUR, ipPC, idlepos[0],idlepos[1],idlepos[2],idlepos[3],idlepos[4],idlepos[5], a, v);

        URControl::RG2(ipUR, ipPC, 0);
    }



};

#endif // URCONTROLPLANE_HPP

